﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Collections;

namespace CPF_experement
{
    static class A_star
    {
        static WorldState root;
        public static bool diagonal;
        public static BinaryHeap<WorldState> openList;
        public static HashSet<WorldState> closedList;
        public static HashSet<WorldState> closedListExpanded;
        public static int expanded;
        public static int generated;
        public static int totalTime;

        static public void init_A_Star( AgentState[] allAgentsState, bool i_diagonal)
        {
            diagonal = i_diagonal;
            closedList = new HashSet<WorldState>();
            closedListExpanded = new HashSet<WorldState>();
            root = new WorldState(allAgentsState);
            openList=new BinaryHeap<WorldState>();
            closedList.Add(root);
            openList.Add(root);
            expanded = 0;
            generated = 0;
            totalTime = 0;
        }

        public static bool solve()
        {
            WorldState temp;
            while (openList.Count > 0)
            {
                if (Run.watch.ElapsedMilliseconds > 120000)
                {
                    totalTime = -2;
                    Console.WriteLine("Expanded - " + expanded);
                    Console.WriteLine("Generated - " + generated);
                    Console.WriteLine("Out of time");
                    return false;
                }
                temp = openList.Remove();
                
                //if (temp.totalTime_G==7 && temp.huristic_H==13 && temp.allAgentsState[0].pos_X==0  && temp.allAgentsState[0].pos_Y==2 && temp.allAgentsState[1].pos_X==1  && temp.allAgentsState[1].pos_Y==1 && temp.allAgentsState[2].pos_X==0  && temp.allAgentsState[2].pos_Y==1 && temp.allAgentsState[3].pos_X==1  && temp.allAgentsState[3].pos_Y==0 && temp.allAgentsState[4].pos_X==1  && temp.allAgentsState[4].pos_Y==2 && temp.allAgentsState[5].pos_X==2  && temp.allAgentsState[5].pos_Y==2 && temp.allAgentsState[6].pos_X==2  && temp.allAgentsState[6].pos_Y==0 && temp.allAgentsState[7].pos_X==0  && temp.allAgentsState[7].pos_Y==0  )
                //{
                //    bool t = true;
                //}
                if (temp.goalTest())
                {
                    totalTime = temp.totalTime_G;
                    printSolution(temp);
                    return true;
                }
                if (!closedListExpanded.Contains(temp))
                {
                    closedListExpanded.Add(temp);
                    expanded++;
                    temp.expandNode(0);
                }
            }
            totalTime = -1;
            Console.WriteLine("Expanded - " + expanded);
            Console.WriteLine("Generated - " + generated);
            Console.WriteLine("No solution found");
            return false;
        }
        private static void printSolution(WorldState end)
        {
            Console.WriteLine("Expanded - "+expanded);
            Console.WriteLine("Generated - " + generated);
            Console.WriteLine("Total Cost - " + totalTime);

            //int step = 0;
            //Console.WriteLine("solution back to front");
            //while (end != null)
            //{
            //    Console.WriteLine("step " + step);
            //    Console.WriteLine(end.ToString());
            //    step++;
            //    end = end.prevStep;
            //}
        }
    }
}

